/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.ADXL345_I2C;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends RobotBase {
    Joystick Drive;
    Victor FR, FL, RR, RL;
    AxisCamera Camera;
    int state, imageCount = 0;
    final int pressed = 0, unpressed = 1;
    long start, totalTime;
    ADXL345_I2C RobotAccelerometer = new ADXL345_I2C(1,ADXL345_I2C.DataFormat_Range.k4G);
    public void startCompetition() {
        double accelX;
        double accelY;
        double accelZ;
        Drive = new Joystick(1);
        Camera = AxisCamera.getInstance();
        FR = new Victor(1, 1);
        FL = new Victor(1, 2);
        RR = new Victor(1, 3);
        RL = new Victor(1, 4);
        while(true){
            switch(state){
                case pressed:
                    if(!Drive.getRawButton(1)){
                        state = unpressed;
                    }
                    break;
                case unpressed:
                    if(Drive.getRawButton(1)){
                        try{
                            start = System.currentTimeMillis();
                            ColorImage image = Camera.getImage();
                            BinaryImage thresholdImage = image.thresholdRGB(0, 138, 172, 255, 132, 255);
                            BinaryImage convexHullImage = thresholdImage.convexHull(false);
                            BinaryImage filteredImage = convexHullImage.removeSmallObjects(true, 5);
                            ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports();
                            image.free();
                            thresholdImage.free();
                            convexHullImage.free();
                            totalTime = System.currentTimeMillis() - start;
                            accelX = RobotAccelerometer.getAcceleration(ADXL345_I2C.Axes.kX);
                            accelY = RobotAccelerometer.getAcceleration(ADXL345_I2C.Axes.kY);
                            accelZ = RobotAccelerometer.getAcceleration(ADXL345_I2C.Axes.kZ);
                            System.out.println("Total Time Elapsed: " + totalTime);
                            System.out.println("Accleration in X is:" +accelX +", Acceleration in the Y is: " +accelY +", Acceleration in the Z is: " +accelZ);
                        } catch(Exception e){
                            //ignore
                        }
                        state = pressed;
                    }
                    break;
            }
            double x, y, z;
            x = Drive.getX();
            y = Drive.getY();
            z = Drive.getZ();
            FR.set(-(y - x + z));
            RR.set(-(y + x + z));
            FL.set(y + x - z);
            RL.set(y - x - z);
            
        }
    }
}
